/*
 * File Name:               XCapsule2D.cs
 * 
 * Description:             普通类
 * Author:                  lisiyu <576603306@qq.com>

 * Create Date:             2017/06/08
 */

public struct XCapsule2D 
{
    public XVector2 Point01;
    public XVector2 Point02;
    public XNumber Radius;

    private XVector2 _right;
    private XVector2 _up;
    private XNumber _right_x;

    public XCapsule2D(XVector2 pos01, XVector2 pos02, XNumber radius)
    {
        Point01 = pos01;
        Point02 = pos02;
        Radius = radius;

        // 建立自身坐标系 point01为原点 到point02的方向为右方向
        _right = (Point02 - Point01).normalized;
        _up = new XVector2(-_right.y, _right.x);
        _right_x = (Point02 - Point01).magnitude;
    }

    public bool cross(XVector2 segmentPos01, XVector2 segmentPos02, out XVector2 crossPos01, out XVector2 crossPos02)
    {
        crossPos01 = XVector2.zero;
        crossPos02 = XVector2.zero;

        var localPos01 = new XVector2(XVector2.Dot(segmentPos01 - Point01, _right), XVector2.Dot(segmentPos01 - Point01, _up));
        var localPos02 = new XVector2(XVector2.Dot(segmentPos02 - Point01, _right), XVector2.Dot(segmentPos02 - Point01, _up));

        if (localPos01.y > Radius && localPos02.y > Radius)
            return false;

        if (localPos01.y < -Radius && localPos02.y < -Radius)
            return false;

        if (localPos01.x < -Radius && localPos02.x < -Radius)
            return false;

        if (localPos01.x > _right_x + Radius && localPos02.x > _right_x + Radius)
            return false;

        XNumber tmpScale01 = XNumber.zero;
        XNumber tmpScale02 = XNumber.zero;

        XVector2 tmpPos01, tmpPos02;
        var dir = (localPos02 - localPos01).normalized;
        var yScale = dir.y.raw == 0 ? 1 : XNumber.one / XIntMath.Abs(dir.y);
        var up = dir.y >= 0;      // 向上的趋势
        if(up)
        {
            if(localPos01.y < -Radius)
            {
                tmpScale01 = (-Radius - localPos01.y) * yScale;
                tmpPos01 = localPos01 + dir * tmpScale01;
            }
            else
            {
                tmpPos01 = localPos01;
            }

            if(localPos02.y > Radius)
            {
                tmpScale02 = -(localPos02.y - Radius) * yScale;
                tmpPos02 = localPos02 + dir * tmpScale02;
            }
            else
            {
                tmpPos02 = localPos02;
            }
        }
        else
        {
            if (localPos01.y > Radius)
            {
                tmpScale01 = (localPos01.y - Radius) * yScale;
                tmpPos01 = localPos01 + dir * tmpScale01;
            }
            else
            {
                tmpPos01 = localPos01;
            }

            if (localPos02.y < -Radius)
            {
                tmpScale02 = -(-Radius - localPos02.y) * yScale;
                tmpPos02 = localPos02 + dir * tmpScale02;
            }
            else
            {
                tmpPos02 = localPos02;
            }
        }


        var realDir = (segmentPos02 - segmentPos01).normalized;

        //if (tmpPos01.x < 0)
        //{
        //    if (tmpPos01.magnitude > Radius)
        //        return false;
        //}
        //else if(tmpPos01.x >= _right_x)
        //{
        //    if (XVector2.Distance(tmpPos01, new XVector2(_right_x, XNumber.zero)) > Radius)
        //        return false;
        //}

        //if (tmpPos02.x < 0)
        //{
        //    if (tmpPos02.magnitude > Radius)
        //        return false;
        //}
        //else if (tmpPos02.x >= _right_x)
        //{
        //    if (XVector2.Distance(tmpPos02, new XVector2(_right_x, XNumber.zero)) > Radius)
        //        return false;
        //}

        if(tmpScale01 == XNumber.zero)
        {
            crossPos01 = segmentPos01;
        }
        else
        {
            crossPos01 = segmentPos01 + realDir * tmpScale01;
        }

        if (tmpScale02 == XNumber.zero)
        {
            crossPos02 = segmentPos02;
        }
        else
        {
            crossPos02 = segmentPos02 + realDir * tmpScale02;
        }

        //DebugExtension.DebugCapsule(Point01.x0z, Point02.x0z, Radius);
        //DebugExtension.DebugArrow(segmentPos01.x0z, segmentPos02.x0z - segmentPos01.x0z, Color.yellow);
        //DebugExtension.DebugArrow(crossPos01.x0z, crossPos02.x0z - crossPos01.x0z, Color.black);
        //Debug.Break();

        return true;
    }

    public void draw()
    {
        DebugExtension.DebugCapsule(Point01.x0z, Point02.x0z, Radius);
    }
}
